#include "postprocess/kalman_filter/position_kalmanfilter.h"

PositionKalmanFilter::PositionKalmanFilter() {}
PositionKalmanFilter::~PositionKalmanFilter() {}

void PositionKalmanFilter::Init(int dynam_param_num, int measure_param_num,
                                int control_param_num) {
  BaseKalmanFilter::Init(dynam_param_num, measure_param_num, control_param_num);
  age_ = 0;
}

//Eigen::VectorXf MotionKalmanFilter::Predict(double time_diff) {
//  Eigen::VectorXf track_predict;
//  track_predict.resize(7);
//  track_predict(0) = belief_center_point_(0) + belief_velocity_(0) * time_diff;
//  track_predict(1) = belief_center_point_(1) + belief_velocity_(1) * time_diff;
//  track_predict(2) = belief_center_point_(2) + belief_velocity_(2) * time_diff;
//  track_predict(3) = belief_velocity_(0);
//  track_predict(4) = belief_velocity_(1);
//  track_predict(5) = belief_velocity_(2);
//  track_predict(6) = time_diff;
//  return track_predict;
//}

void PositionKalmanFilter::SetCenterPoint(Eigen::Vector3f point) noexcept {
  belief_center_point_ = point;
  // State_.head<3>() = point;
}

void PositionKalmanFilter::SetVelocity(Eigen::Vector3f vel) noexcept {
  belief_velocity_ = vel;
  // State_.segment(3, 2) = vel.head<2>();
}

void PositionKalmanFilter::SetAcceleration(Eigen::Vector3f acc) noexcept {
  belief_acceleration_ = acc;
  // State_.tail<2>() = acc.head<2>();
}

void PositionKalmanFilter::SetQ(Eigen::MatrixXf& Q) {}

void PositionKalmanFilter::SetR(Eigen::MatrixXf& R) {}

void PositionKalmanFilter::GetState(Eigen::Vector3f* center_point, float vx,
                                    float vy, float ax, float ay) noexcept {}

void PositionKalmanFilter::UpdateWithObject(TrackedObjectPtr& new_object,
    TrackedObjectPtr const& old_object,
    double time_diff) {
  // kalman filter update
  update_state(new_object, old_object, time_diff);
  age_ += 1;
}

void PositionKalmanFilter::UpdateWithoutObject(double time_diff) {
  belief_center_point_ += belief_velocity_ * time_diff;
  age_ += 1;
}

void PositionKalmanFilter::update_state(TrackedObjectPtr const& new_object,
                                        TrackedObjectPtr const& old_object,
                                        double time_diff) {
  // A赋值
  A_(0, 3) = time_diff;
  A_(1, 4) = time_diff;
  A_(3, 5) = time_diff;
  A_(4, 6) = time_diff;
  A_(0, 5) = time_diff * time_diff;
  A_(1, 6) = time_diff * time_diff;

  // to do
  // kalman的P、Q、R赋值

  // to do
  //  measurement
  Eigen::VectorXf Z = Eigen::VectorXf::Zero(dynam_param_num_);
  // update
  // x = A * x + B * u
  State_ = A_ * State_;
  // p = A * p * A^T + Q
  P_ = A_ * P_ * A_.transpose() + Q_;
  // K = p * H^T / (H * p * H^T + R)
  auto K = P_ * H_.transpose() * (H_ * P_ * H_.transpose() + R_);
  // x= x + K * (z - H * x)
  State_ = State_ + K * (Z - H_ * State_);
  // p = (I - K * H) * p
  Eigen::MatrixXf I =
    Eigen::MatrixXf::Identity(dynam_param_num_, dynam_param_num_);
  P_ = (I - K * H_) * P_;

  // assign value
  belief_center_point_ = State_.head<3>();
  belief_velocity_.head<2>() = State_.segment(3, 2);
  belief_velocity_(2) = 0.0f;
  belief_acceleration_.head<2>() = State_.segment(5, 2);
  belief_acceleration_(2) = 0.0f;
}
